[x_{a2}\\y_{a2}]=[L+l_1 c_{a1} +l_2 c_{a1+a2}\\l_1 s_{a1} +l_2 s_{a1+a2}]
[x_{b2}\\y_{b2}]=[-L+l_1 c_{b1} +l_2 c_{b1+b2}\\l_1 s_{b1} +l_2 s_{b1+b2}]

[x_3\\y_3] 
+\pi/2.

(x_{b2}-x_{a2})^2+(y_{b2}-y_{a2})^2=l_3^2.

 [x_3,y_3,\theta_3]
